
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_attitude_control_multi.c
  * @author     baiyang
  * @date       2021-8-9
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <string.h>

#include "mc_attitude_control_multi.h"

#include <parameter/param.h>
#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   att_ctrl  
  * @param[in]   motors  
  * @param[in]   dt  
  * @param[out]  
  * @retval      
  * @note        
  */
void attctrl_multi_init(Attitude_ctrl * att_ctrl, Motors_HandleTypeDef* motors, float dt)
{
    AttitudeMulti_ctrl* att_ctrl_multi = (AttitudeMulti_ctrl*)att_ctrl;

    attctrl_init(att_ctrl, motors, dt);

    att_ctrl_multi->_motors = (MotorsMC_HandleTypeDef*)motors;

    pid_ctrl_ctor(&att_ctrl_multi->_pid_rate_roll, 
                           AC_ATC_MULTI_RATE_RP_P,
                           AC_ATC_MULTI_RATE_RP_I,
                           AC_ATC_MULTI_RATE_RP_D,
                           0.0f,
                           AC_ATC_MULTI_RATE_RP_IMAX,
                           AC_ATC_MULTI_RATE_RP_FILT_HZ,
                           0.0f,
                           AC_ATC_MULTI_RATE_RP_FILT_HZ,
                           dt);
    pid_ctrl_ctor(&att_ctrl_multi->_pid_rate_pitch, 
                           AC_ATC_MULTI_RATE_RP_P,
                           AC_ATC_MULTI_RATE_RP_I,
                           AC_ATC_MULTI_RATE_RP_D,
                           0.0f,
                           AC_ATC_MULTI_RATE_RP_IMAX,
                           AC_ATC_MULTI_RATE_RP_FILT_HZ,
                           0.0f,
                           AC_ATC_MULTI_RATE_RP_FILT_HZ,
                           dt);
    pid_ctrl_ctor(&att_ctrl_multi->_pid_rate_yaw, 
                           AC_ATC_MULTI_RATE_YAW_P,
                           AC_ATC_MULTI_RATE_YAW_I,
                           AC_ATC_MULTI_RATE_YAW_D,
                           0.0f,
                           AC_ATC_MULTI_RATE_YAW_IMAX,
                           AC_ATC_MULTI_RATE_RP_FILT_HZ,
                           AC_ATC_MULTI_RATE_YAW_FILT_HZ,
                           0.0f,
                           dt);

    att_ctrl_multi->_thr_mix_man = AC_ATTITUDE_CONTROL_MAN_DEFAULT;
    att_ctrl_multi->_thr_mix_min = AC_ATTITUDE_CONTROL_MIN_DEFAULT;
    att_ctrl_multi->_thr_mix_max = AC_ATTITUDE_CONTROL_MAX_DEFAULT;

    // 加载参数
    attctrl_parameter_sanity_check(att_ctrl);
    attctrl_update_param(att_ctrl);
}

// Update Alt_Hold angle maximum
void attctrl_update_althold_lean_angle_max(Attitude_ctrl * att_ctrl, float throttle_in)
{
    AttitudeMulti_ctrl* att_ctrl_multi = (AttitudeMulti_ctrl*)att_ctrl;

    // calc maximum tilt angle based on throttle
    float thr_max = att_ctrl_multi->_motors->_throttle_thrust_max;

    // divide by zero check
    if (math_flt_zero(thr_max)) {
        att_ctrl->_althold_lean_angle_max = 0.0f;
        return;
    }

    float althold_lean_angle_max = acosf(math_constrain_float(throttle_in / (AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f));
    att_ctrl->_althold_lean_angle_max = att_ctrl->_althold_lean_angle_max + (att_ctrl->_dt / (att_ctrl->_dt + att_ctrl->_angle_limit_tc)) * (althold_lean_angle_max - att_ctrl->_althold_lean_angle_max);
}

void attctrl_set_throttle_out(Attitude_ctrl * att_ctrl, float throttle_in, bool apply_angle_boost, float filter_cutoff)
{
    att_ctrl->_throttle_in = throttle_in;
    attctrl_update_althold_lean_angle_max(att_ctrl, throttle_in);
    Motors_set_throttle_filter_cutoff(att_ctrl->_motors, filter_cutoff);
    if (apply_angle_boost) {
        // Apply angle boost
        throttle_in = attctrl_get_throttle_boosted(att_ctrl, throttle_in);
    } else {
        // Clear angle_boost for logging purposes
        att_ctrl->_angle_boost = 0.0f;
    }

    Motors_set_throttle(att_ctrl->_motors,throttle_in);
    Motors_set_throttle_avg_max(att_ctrl->_motors, attctrl_get_throttle_avg_max(att_ctrl,MAX(throttle_in, att_ctrl->_throttle_in)));
}

void attctrl_set_throttle_mix_max(Attitude_ctrl * att_ctrl, float ratio)
{
    AttitudeMulti_ctrl* att_ctrl_multi = (AttitudeMulti_ctrl*)att_ctrl;

    ratio = math_constrain_float(ratio, 0.0f, 1.0f);
    att_ctrl->_throttle_rpy_mix_desired = (1.0f - ratio) * att_ctrl_multi->_thr_mix_min + ratio * att_ctrl_multi->_thr_mix_max;
}

// returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1
float attctrl_get_throttle_boosted(Attitude_ctrl * att_ctrl, float throttle_in)
{
    if (!att_ctrl->_angle_boost_enabled) {
        att_ctrl->_angle_boost = 0;
        return throttle_in;
    }
    // inverted_factor is 1 for tilt angles below 60 degrees
    // inverted_factor reduces from 1 to 0 for tilt angles between 60 and 90 degrees
    Euler_t attitude_euler;
    quat_to_euler(&att_ctrl->_att_data.attitude_body, &attitude_euler);

    float cos_tilt = cosf(attitude_euler.pitch) * cosf(attitude_euler.roll);
    float inverted_factor = math_constrain_float(10.0f * cos_tilt, 0.0f, 1.0f);
    float cos_tilt_target = cosf(att_ctrl->_thrust_angle);
    float boost_factor = 1.0f / math_constrain_float(cos_tilt_target, 0.1f, 1.0f);

    float throttle_out = throttle_in * inverted_factor * boost_factor;
    att_ctrl->_angle_boost = math_constrain_float(throttle_out - throttle_in, -1.0f, 1.0f);
    return throttle_out;
}

// returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1
float attctrl_get_throttle_avg_max(Attitude_ctrl * att_ctrl, float throttle_in)
{
    AttitudeMulti_ctrl* att_ctrl_multi = (AttitudeMulti_ctrl*)att_ctrl;

    throttle_in = math_constrain_float(throttle_in, 0.0f, 1.0f);
    return MAX(throttle_in, throttle_in * MAX(0.0f, 1.0f - att_ctrl->_throttle_rpy_mix) + att_ctrl_multi->_motors->_throttle_hover * att_ctrl->_throttle_rpy_mix);
}

/**
  * @brief       
  * @param[in]   att_ctrl  
  * @param[out]  
  * @retval      
  * @note        
  */
void attctrl_rate_controller_run(Attitude_ctrl * att_ctrl)
{
    AttitudeMulti_ctrl* att_ctrl_multi = (AttitudeMulti_ctrl*)att_ctrl;

    // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
    attctrl_update_throttle_rpy_mix(att_ctrl);

    vec3_add(&att_ctrl->_ang_vel_body, &att_ctrl->_ang_vel_body, &att_ctrl->_sysid_ang_vel_body);

    Vector3f_t gyro_latest = att_ctrl->_att_data.gyro;

    Motors_set_roll(att_ctrl->_motors,pid_ctrl_update_all(&att_ctrl_multi->_pid_rate_roll, 
        att_ctrl->_ang_vel_body.x, gyro_latest.x, att_ctrl->_motors->limit.roll) + att_ctrl->_actuator_sysid.x);
    Motors_set_roll_ff(att_ctrl->_motors, pid_ctrl_get_ff(&att_ctrl_multi->_pid_rate_roll));

    Motors_set_pitch(att_ctrl->_motors,pid_ctrl_update_all(&att_ctrl_multi->_pid_rate_pitch, 
        att_ctrl->_ang_vel_body.y, gyro_latest.y, att_ctrl->_motors->limit.pitch) + att_ctrl->_actuator_sysid.y);
    Motors_set_pitch_ff(att_ctrl->_motors, pid_ctrl_get_ff(&att_ctrl_multi->_pid_rate_pitch));

    Motors_set_yaw(att_ctrl->_motors,pid_ctrl_update_all(&att_ctrl_multi->_pid_rate_yaw, 
        att_ctrl->_ang_vel_body.z, gyro_latest.z, att_ctrl->_motors->limit.yaw) + att_ctrl->_actuator_sysid.z);
    Motors_set_yaw_ff(att_ctrl->_motors, pid_ctrl_get_ff(&att_ctrl_multi->_pid_rate_yaw));

    vec3_zero(&att_ctrl->_sysid_ang_vel_body);
    vec3_zero(&att_ctrl->_actuator_sysid);

    attctrl_publish_msg(att_ctrl);
}

// update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value
void attctrl_update_throttle_rpy_mix(Attitude_ctrl * att_ctrl)
{
    // slew _throttle_rpy_mix to _throttle_rpy_mix_desired
    if (att_ctrl->_throttle_rpy_mix < att_ctrl->_throttle_rpy_mix_desired) {
        // increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds)
        att_ctrl->_throttle_rpy_mix += MIN(2.0f * att_ctrl->_dt, att_ctrl->_throttle_rpy_mix_desired - att_ctrl->_throttle_rpy_mix);
    } else if (att_ctrl->_throttle_rpy_mix > att_ctrl->_throttle_rpy_mix_desired) {
        // reduce more slowly (from 0.9 to 0.1 in 1.6 seconds)
        att_ctrl->_throttle_rpy_mix -= MIN(0.5f * att_ctrl->_dt, att_ctrl->_throttle_rpy_mix - att_ctrl->_throttle_rpy_mix_desired);
    }
    att_ctrl->_throttle_rpy_mix = math_constrain_float(att_ctrl->_throttle_rpy_mix, 0.1f, AC_ATTITUDE_CONTROL_MAX);
}

void attctrl_reset_rate_controller_I_terms(Attitude_ctrl * att_ctrl)
{
    AttitudeMulti_ctrl* att_ctrl_multi = (AttitudeMulti_ctrl*)att_ctrl;
    
    pid_ctrl_reset_i(&att_ctrl_multi->_pid_rate_roll);
    pid_ctrl_reset_i(&att_ctrl_multi->_pid_rate_pitch);
    pid_ctrl_reset_i(&att_ctrl_multi->_pid_rate_yaw);
}

// reset rate controller I terms smoothly to zero in 0.5 seconds
void attctrl_reset_rate_controller_I_terms_smoothly(Attitude_ctrl * att_ctrl)
{
    AttitudeMulti_ctrl* att_ctrl_multi = (AttitudeMulti_ctrl*)att_ctrl;
    
    pid_ctrl_reset_i_smoothly(&att_ctrl_multi->_pid_rate_roll);
    pid_ctrl_reset_i_smoothly(&att_ctrl_multi->_pid_rate_pitch);
    pid_ctrl_reset_i_smoothly(&att_ctrl_multi->_pid_rate_yaw);
}

// sanity check parameters.  should be called once before takeoff
void attctrl_parameter_sanity_check(Attitude_ctrl * att_ctrl)
{
    AttitudeMulti_ctrl* att_ctrl_multi = (AttitudeMulti_ctrl*)att_ctrl;

    // sanity check throttle mix parameters
    if (att_ctrl_multi->_thr_mix_man < 0.1f || att_ctrl_multi->_thr_mix_man > 4.0f) {
        // parameter description recommends thr-mix-man be no higher than 0.9 but we allow up to 4.0
        // which can be useful for very high powered copters with very low hover throttle
        PARAM_SET_FLOAT(ATT_CTRL, ATC_THR_MIX_MAN, AC_ATTITUDE_CONTROL_MAN_DEFAULT);
    }
    if (att_ctrl_multi->_thr_mix_min < 0.1f || att_ctrl_multi->_thr_mix_min > 0.25f) {
        PARAM_SET_FLOAT(ATT_CTRL, ATC_THR_MIX_MIN, AC_ATTITUDE_CONTROL_MIN_DEFAULT);
    }
    if (att_ctrl_multi->_thr_mix_max < 0.5f || att_ctrl_multi->_thr_mix_max > AC_ATTITUDE_CONTROL_MAX) {
        // parameter description recommends thr-mix-max be no higher than 0.9 but we allow up to 5.0
        // which can be useful for very high powered copters with very low hover throttle
        PARAM_SET_FLOAT(ATT_CTRL, ATC_THR_MIX_MAX, AC_ATTITUDE_CONTROL_MAX_DEFAULT);
    }
    if (att_ctrl_multi->_thr_mix_min > att_ctrl_multi->_thr_mix_max) {
        PARAM_SET_FLOAT(ATT_CTRL, ATC_THR_MIX_MIN, AC_ATTITUDE_CONTROL_MIN_DEFAULT);
        PARAM_SET_FLOAT(ATT_CTRL, ATC_THR_MIX_MAX, AC_ATTITUDE_CONTROL_MAX_DEFAULT);
    }
}

// are we producing min throttle?
bool attctrl_is_throttle_mix_min(Attitude_ctrl * att_ctrl)
{
    AttitudeMulti_ctrl* att_ctrl_multi = (AttitudeMulti_ctrl*)att_ctrl;

    return (att_ctrl->_throttle_rpy_mix < 1.25f * att_ctrl_multi->_thr_mix_min); 
}

/**
  * @brief       更新姿态和陀螺仪数据，用于姿态控制
  * @param[in]   pAttCtrlMulti  
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        此函数应该在运行attctrl_input_euler_angle_roll_pitch_euler_rate_yaw之前调用,
  *              且更新频率要和attctrl_rate_controller_run函数的更新频率一致
  */
void attctrl_updata_att_data(Attitude_ctrl * att_ctrl)
{
    uitc_sensor_gyr sensor_gyr = {0};
    itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr);

    uitc_vehicle_attitude vehicle_attitude;
    itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude);

    memcpy(&att_ctrl->_att_data.gyro, sensor_gyr.sensor_gyr_filter, sizeof(sensor_gyr.sensor_gyr_filter));
    att_ctrl->_att_data.attitude_body = vehicle_attitude.vehicle_quat;
}

/**
  * @brief       发布姿态控制相关数据
  * @param[in]   att_ctrl  
  * @param[out]  
  * @retval      
  * @note        
  */
void attctrl_publish_msg(Attitude_ctrl * att_ctrl)
{
    uitc_vehicle_att_ctrl uitc_att_ctrl = {0};

    uitc_att_ctrl.timestamp_us = time_micros64();

    uitc_att_ctrl.target_euler_angle[0] = att_ctrl->_euler_angle_target.x;    // 目标欧拉角，x:roll，y:pitch，z:yaw
    uitc_att_ctrl.target_euler_angle[1] = att_ctrl->_euler_angle_target.y;    // 目标欧拉角，x:roll，y:pitch，z:yaw
    uitc_att_ctrl.target_euler_angle[2] = att_ctrl->_euler_angle_target.z;    // 目标欧拉角，x:roll，y:pitch，z:yaw
    
    uitc_att_ctrl.target_ang_vel_body[0] = att_ctrl->_ang_vel_target.x;   // 目标轴角速率，小角度时，轴角速率约等于三轴角速率
    uitc_att_ctrl.target_ang_vel_body[1] = att_ctrl->_ang_vel_target.y;   // 目标轴角速率，小角度时，轴角速率约等于三轴角速率
    uitc_att_ctrl.target_ang_vel_body[2] = att_ctrl->_ang_vel_target.z;   // 目标轴角速率，小角度时，轴角速率约等于三轴角速率

    uitc_att_ctrl.throttle_in = att_ctrl->_motors->_throttle_in;          // 输入油门

    PID_ctrl* r_rate_pid = attctrl_get_rate_roll_pid(att_ctrl);
    PID_ctrl* p_rate_pid = attctrl_get_rate_pitch_pid(att_ctrl);
    PID_ctrl* y_rate_pid = attctrl_get_rate_yaw_pid(att_ctrl);

    // 姿态内环PID目标和误差
    uitc_att_ctrl.ctrl_r_pid_target = r_rate_pid->target;
    uitc_att_ctrl.ctrl_r_pid_error  = r_rate_pid->error;
    uitc_att_ctrl.ctrl_p_pid_target = p_rate_pid->target;
    uitc_att_ctrl.ctrl_p_pid_error  = p_rate_pid->error;
    uitc_att_ctrl.ctrl_y_pid_target = y_rate_pid->target;
    uitc_att_ctrl.ctrl_y_pid_error  = y_rate_pid->error;

    itc_publish(ITC_ID(vehicle_att_ctrl), &uitc_att_ctrl);
}

/*------------------------------------test------------------------------------*/


